using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet
{
	public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB);

	public class ManifoldResult : DiscreteCollisionDetectorInterface.Result
	{
		private PersistentManifold _manifold;
		private static ContactAddedCallback _contactAddedCallback = null;

		//we need this for compounds
		private Matrix _rootTransA;
		private Matrix _rootTransB;

		private CollisionObject _bodyA;
		private CollisionObject _bodyB;
		private int _partIdA;
		private int _partIdB;
		private int _indexA;
		private int _indexB;

		public ManifoldResult()
		{
		}

		public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)
		{
			_bodyA = bodyA;
			_bodyB = bodyB;
			_rootTransA = bodyA.WorldTransform;
			_rootTransB = bodyB.WorldTransform;
		}

		public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } }

		public void SetPersistentManifold(PersistentManifold manifold)
		{
			_manifold = manifold;
		}

		public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
		{
			_partIdA = partIdA;
			_partIdB = partIdB;
			_indexA = indexA;
			_indexB = indexB;
		}

        public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
		{
			if (_manifold == null)
				throw new BulletException("Manifold Pointer is null.");

			//order in manifold needs to match

			if (depth > PersistentManifold.ContactBreakingThreshold)
				return;

			bool isSwapped = _manifold.BodyA != _bodyA;

            Matrix transAInv = isSwapped ? MathHelper.InvertMatrix(_rootTransB) : MathHelper.InvertMatrix(_rootTransA);
            Matrix transBInv = isSwapped ? MathHelper.InvertMatrix(_rootTransA) : MathHelper.InvertMatrix(_rootTransB);

			Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
			Vector3 localA = MathHelper.MatrixToVector(transAInv, pointA);
			Vector3 localB = MathHelper.MatrixToVector(transBInv, pointInWorld);
			ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth);

			int insertIndex = _manifold.GetCacheEntry(newPt);

			newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB);
			newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB);

			//User can override friction and/or restitution
			if (_contactAddedCallback != null &&
				//and if either of the two bodies requires custom material
				 ((_bodyA.CollisionFlags & CollisionFlags.CustomMaterialCallback) != 0 ||
				   (_bodyB.CollisionFlags & CollisionFlags.CustomMaterialCallback) != 0))
			{
				//experimental feature info, for per-triangle material etc.
				CollisionObject obj0 = isSwapped ? _bodyB : _bodyA;
				CollisionObject obj1 = isSwapped ? _bodyA : _bodyB;
				_contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB);
			}

			if (insertIndex >= 0)
			{
				_manifold.ReplaceContactPoint(newPt, insertIndex);
			}
			else
			{
				_manifold.AddManifoldPoint(newPt);
			}
		}

		private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB)
		{
			float friction = bodyA.Friction * bodyB.Friction;

			float MaxFriction = 10;
			if (friction < -MaxFriction)
				friction = -MaxFriction;
			if (friction > MaxFriction)
				friction = MaxFriction;
			return friction;
		}

		private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB)
		{
			return bodyA.Restitution * bodyB.Restitution;
		}
	}
}
